#include "Separation.h"
#include "Agent.h"
#include "SteeringOutput.h"
#include "AgentGroup.h"
#include <algorithm>
#include "MyGL.h"

namespace Engine
{
	Separation::Separation():m_threshold(0.0f), m_thresholdSquared(0.0f), m_decayCoefficient(0.0f){}

	void Separation::Update(SteeringOutput* output)
	{
		AgentGroup* g = m_parent ->GetGroup();
		Agents neighbours;

		unsigned int numNeighbours = neighbours.size();

		Vec3 direction, parentPos;
		parentPos = m_parent ->GetPos();
		float strength, maxAcc;
		strength = 0.0f;
		maxAcc = m_parent ->GetMaxAcc();
		
		for(auto it = g ->GetNeighbours().begin(); it != g ->GetNeighbours().end(); ++it)
		{
			if(it ->second ->GetGameObjectID() == m_parent ->GetGameObjectID()) continue;

			direction = it ->second ->GetPos() - parentPos;
			strength = std::min <float> (m_decayCoefficient / direction.SquaredLength(), maxAcc);

			direction.Normalize();
			output ->linear += direction * strength;
		}
	}

	void Separation::Draw()
	{
		Vec3 pos = m_parent ->GetPos();

		glPushMatrix();
			glBegin(GL_LINE_STRIP);
				glColor3f(1.0f, 0.0f, 0.0f);//red
				float angle = 0.0f;
				while(angle < 6.3f)
				{
					glVertex3f(pos.x + (std::cos(angle) * m_threshold), 
						       0.0f, 
							   pos.z + (std::sin(angle) * m_threshold));

					angle += .1f;
				}
			glEnd();
		glPopMatrix();
	}
}